#include <memory>
#include <vector>
#include <string>
#include <cmath>
#include <optional>
#include <sstream>
#include <iomanip>  // 控制小数位数
#include "rclcpp/rclcpp.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
#include "builtin_interfaces/msg/duration.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "rclcpp/parameter_event_handler.hpp"
#include "virtual_arm_interfaces/srv/set_arm.hpp"

using JointTrajectory = trajectory_msgs::msg::JointTrajectory;
using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
using JointState = sensor_msgs::msg::JointState;
using SetArm = virtual_arm_interfaces::srv::SetArm;

class ArmServer:public rclcpp::Node{
private:
    rclcpp::Publisher<JointTrajectory>::SharedPtr joint_position_publisher_; // joint_position发布者
    rclcpp::Service<SetArm>::SharedPtr set_position_service_; // 设置角度服务
    rclcpp::Subscription<JointState> ::SharedPtr joint_state_subscripter_; // 当前joint_state订阅者

    OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; // 添加参数更改回调函数返回句柄

    rclcpp::Subscription<JointState>::SharedPtr joint_subscripter_; // joint_position 话题订阅者，用于获取当前角度

    rclcpp::TimerBase::SharedPtr init_postion_timer_; // 初始化角度一次性定时器

    bool got_joint_state_once_{false}; // 是否收到过订阅的JointState的信息判断
    double joint1_position{0.0},joint2_position{0.0},joint3_position{0.0},joint4_position{0.0};
    
    // 参数事件监听
    std::shared_ptr<rclcpp::ParameterEventHandler> param_event_handler_;
    rclcpp::ParameterCallbackHandle::SharedPtr cb_j1_,cb_j2_ , cb_j3_,cb_j4_;

    // 待应用的命令缓存
    std::optional<double> pending_j1_;
    std::optional<double> pending_j2_;
    std::optional<double> pending_j3_;
    std::optional<double> pending_j4_;

    // 更改joint_position参数的一次性定时器
    rclcpp::TimerBase::SharedPtr joint_param_apply_once_timer_;

    // 把服务和定时器放在同一个互斥回调组，防止并发踩状态
    rclcpp::CallbackGroup::SharedPtr cbgroup_one_;
public:
    ArmServer():Node("arm_server"){
        // 设置参数
        declare_parameter<std::vector<std::string>>("arm_joint_names", {"joint1","joint2","joint3","joint4"}); //arm 关节名称
        declare_parameter<double>("joint1_position",0.0);
        declare_parameter<double>("joint2_position",0.0);
        declare_parameter<double>("joint3_position",0.0);
        declare_parameter<double>("joint4_position",0.0);
        declare_parameter<double>("move_time",1.0); // 运动历经时间
        declare_parameter<double>("start_delay",0.2); // 开始时间的延时

        // 互斥回调组
        this->cbgroup_one_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

        // Qos 用来定义消息传输的规则 有很多历史策略、可靠性。。。
        rclcpp::QoS qos(rclcpp::KeepLast(1)); //历史策略：只保留最新的一条信息
        qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); //可靠性：必须可靠

        // 位置发布者
        this->joint_position_publisher_ = this->create_publisher<JointTrajectory>("/arm_controller/joint_trajectory",qos);
        
        // 设置position 服务
        this->set_position_service_ = this->create_service<SetArm>("/arm/set",std::bind(&ArmServer::SetArmPosition,this,std::placeholders::_1,std::placeholders::_2),rmw_qos_profile_services_default,cbgroup_one_);

        // 参数更改回调
        this->parameter_callback_handle_ = this->add_on_set_parameters_callback(std::bind(&ArmServer::paramCallback,this,std::placeholders::_1));

        // 指定的joint_position参数改变事件回调（异步）
        this->param_event_handler_ = std::make_shared<rclcpp::ParameterEventHandler>(this);

        auto create_cb = [this](rclcpp::ParameterCallbackHandle::SharedPtr& handle, const std::string& param_name, std::function<void(double)> on_value) {
            handle = this->param_event_handler_->add_parameter_callback(
                param_name,
                [this, param_name, on_value](const rclcpp::Parameter & p) {
                    if (p.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) {
                        on_value(p.as_double());
                    } else {
                        RCLCPP_WARN(this->get_logger(),
                        "%s expects double, got %s", param_name.c_str(), p.get_type_name().c_str());
                    }
                }
            );
        };


        create_cb(this->cb_j1_, "joint1_position",
                [this](double v){ this->request_publish_from_params(v, std::nullopt, std::nullopt, std::nullopt); });
        create_cb(this->cb_j2_, "joint2_position",
                [this](double v){ this->request_publish_from_params(std::nullopt, v, std::nullopt, std::nullopt); });
        create_cb(this->cb_j3_, "joint3_position",
                [this](double v){ this->request_publish_from_params(std::nullopt, std::nullopt, v, std::nullopt); });
        create_cb(this->cb_j4_, "joint4_position",
                [this](double v){ this->request_publish_from_params(std::nullopt, std::nullopt, std::nullopt, v); });

        // 订阅JointState
        this->joint_subscripter_ = this->create_subscription<JointState>("joint_states",rclcpp::QoS(rclcpp::KeepLast(1)).reliable(),
        [this](JointState::SharedPtr msg){
            this->joint_listener_callback(msg);
            got_joint_state_once_ = true;
        });


        RCLCPP_INFO(get_logger(), "arm_server ready. Services: /arm/set");

        //通过一次性定时器，等待系统就绪后在发布初始化轨迹
        using namespace std::chrono_literals;
        this->init_postion_timer_ = this->create_wall_timer(100ms,std::bind(&ArmServer::init_position_callback,this),this->cbgroup_one_);

    }
private:
    void init_position_callback(){
        //  系统就绪检查（与 init_position_callback 一致）
        bool use_sim_time = false;
        this->get_parameter_or("use_sim_time", use_sim_time, false);
        if (use_sim_time && this->get_clock()->now().nanoseconds() == 0) 
            return;
        if (this->joint_position_publisher_->get_subscription_count() == 0)   
            return;
        if (!this->got_joint_state_once_) return;

        //条件满足开始初始化position，start_delay给足一点
        this->publishSynced(0.0,0.0,0.0,0.0,1.0,0.5);
        RCLCPP_INFO(this->get_logger(), "InitArm sent after system ready.");
        // 初始化后，定时结束
        this->init_postion_timer_->cancel();
    }

    // 订阅角度话题的回调函数
    void joint_listener_callback(const JointState::SharedPtr msg){
        int count = 0;
        for(size_t i = 0;i < msg->name.size();i++){
            if(msg->name[i] == "joint1"){
                this->joint1_position = msg->position[i];
                count++;
                if(count == 4)break;
            }
            if(msg->name[i] == "joint2"){
                this->joint2_position = msg->position[i];
                count++;
                if(count == 4)break;
            }
            if(msg->name[i] == "joint3"){
                this->joint3_position = msg->position[i];
                count++;
                if(count == 4)break;
            }
            if(msg->name[i] == "joint4"){
                this->joint4_position = msg->position[i];
                count++;
                if(count == 4)break;
            }
        }
    }

    // joint_position 参数更改统一入口，负责记录更改的position + 触发一次定时器
    void request_publish_from_params(std::optional<double> j1,std::optional<double> j2,std::optional<double> j3,std::optional<double> j4){
        RCLCPP_INFO(this->get_logger(),"-----2------");
        if (j1) this->pending_j1_ = *j1;
        if (j2) this->pending_j2_ = *j2;
        if (j3) this->pending_j3_ = *j3;
        if (j4) this->pending_j4_ = *j4;
        schedule_apply_once(std::chrono::milliseconds(1));
    }


     // 创建/重置一次行定时器， 定时回调中完成就绪检查与真正的publish
    void schedule_apply_once(std::chrono::milliseconds delay){
        // 如果旧定时器存在，清除它
        if(this->joint_param_apply_once_timer_)         this->joint_param_apply_once_timer_->cancel();

        // 建立新的一次性定时器
        this->joint_param_apply_once_timer_ = this->create_wall_timer(delay,[this](){
            // 判断是否有参数更新
            if(!this->pending_j1_ && !this->pending_j2_ && !this->pending_j3_ && !this->pending_j4_){
                this->joint_param_apply_once_timer_->cancel();
                return ;
            }

            //  系统就绪检查（与 init_position_callback 一致）
            bool use_sim_time = false;
            this->get_parameter_or("use_sim_time", use_sim_time, false);
            if (use_sim_time && this->get_clock()->now().nanoseconds() == 0) 
                return;
            if (this->joint_position_publisher_->get_subscription_count() == 0)   
                return;
            if (!this->got_joint_state_once_) return;

            // 取将要发布的值（未提供的项用当前/默认）
            double j1 = pending_j1_.value_or(this->joint1_position);
            double j2 = pending_j2_.value_or(this->joint2_position);
            double j3 = pending_j3_.value_or(this->joint3_position);
            double j4 = pending_j4_.value_or(this->joint4_position);
            double move_time = get_parameter("move_time").as_double();
            double start_delay = get_parameter("start_delay").as_double();
            
            auto check = [this](std::string joint_name,double j,double minv,double maxv)-> bool 
            {
                if(j < minv || j > maxv){
                    RCLCPP_WARN(this->get_logger(),"Param-triggered: %s_position out of range [%.2lf, %.2lf]",joint_name.c_str(),minv,maxv);
                    this->pending_j1_.reset();
                    this->pending_j2_.reset();
                    this->pending_j3_.reset();
                    this->pending_j4_.reset();
                    this->joint_param_apply_once_timer_->cancel();
                    return false;
                }
                return true;
            };

            if(!check("joint1",j1,-1.58,1.58))return ;
            if(!check("joint2",j2,-1.58,1.58))return ;
            if(!check("joint3",j3,-1.58,1.58))return ;
            if(!check("joint4",j4,-1.58,1.58))return ;

            // 统一发布（与服务同路径）
            this->publishSynced(j1, j2,j3,j4, move_time, start_delay);
            RCLCPP_INFO(this->get_logger(), "Applied param-triggered gripper cmd: j1=%.3f, j2=%.3f, j3=%.3f, j4=%.3f", j1, j2, j3 ,j4);

            // // 清空参数，并结束once timer
            this->pending_j1_.reset();
            this->pending_j2_.reset();
            this->pending_j3_.reset();
            this->pending_j4_.reset();
            this->joint_param_apply_once_timer_->cancel();


        },this->cbgroup_one_); //最后的this->cbgroup_one_ 是将定时器加入互斥回调组，防止服务和定时器并发互踩
    }

    // 设置参数回调函数，当在设置参数的时候调用
    rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & params){
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        result.reason = "success";
        for(const auto & p : params){
        
            if(p.get_name() == "start_delay"){
                double v = p.as_double();
                if(v < 0.0){
                    result.successful = false;
                    result.reason = "start_delay must be bigger than 0 seconds";
                    RCLCPP_WARN(this->get_logger(),"start_delay must be bigger than 0 seconds");
                }
                else{
                    RCLCPP_INFO(this->get_logger(), "Updated start_delay=%.2f", v);
                }
            }

            if(p.get_name() == "move_time"){
                double v = p.as_double();
                if(v < 0.0){
                    result.successful = false;
                    result.reason = "move_time must be bigger than 0 seconds";
                    RCLCPP_WARN(this->get_logger(),"move_time must be bigger than 0 seconds");
                }
                else{
                    RCLCPP_INFO(this->get_logger(), "Updated move_time=%.2f", v);
                }
            }
        }

        return result;
    }

    //将运动历经时间转换为 接口中类型
    static builtin_interfaces::msg::Duration secsToDuration(double s) {
        builtin_interfaces::msg::Duration d;
        if (s < 0) s = 0;
        // floor 向下取整 static_cast 强转
        d.sec = static_cast<int32_t>(std::floor(s));
        d.nanosec = static_cast<uint32_t>((s - std::floor(s)) * 1e9);
        return d;
    }

    void publishSynced(double joint1,double joint2,double joint3,double joint4,double move_time,double start_delay){
         // 先获取rclcpp::Time类型的开始时间
        const auto target_time = this->get_clock()->now() + rclcpp::Duration::from_seconds(start_delay);

        // 将其转化为接口类型的开始时间
        builtin_interfaces::msg::Time t0;
        t0.sec = static_cast<int32_t>(target_time.nanoseconds() / 1000000000);
        t0.nanosec = static_cast<uint32_t>(target_time.nanoseconds() % 1000000000);

        auto arm_joint_names = get_parameter("arm_joint_names").as_string_array();

        // arm 轨迹
        JointTrajectory arm;
        arm.header.stamp = t0;
        arm.joint_names = arm_joint_names;

        JointTrajectoryPoint ap;
        ap.time_from_start = secsToDuration(move_time);
        ap.positions.resize(arm_joint_names.size(),0.0);
        for(size_t i = 0;i < arm_joint_names.size();i++){
            if(arm_joint_names[i] == "joint1")ap.positions[i] = joint1;
            else if(arm_joint_names[i] == "joint2")ap.positions[i] = joint2;
            else if(arm_joint_names[i] == "joint3")ap.positions[i] = joint3;
            else if(arm_joint_names[i] == "joint4")ap.positions[i] = joint4;
        }
        arm.points.push_back(ap);
        this->joint_position_publisher_->publish(arm);
        RCLCPP_INFO(this->get_logger(), "Published synced arm trajectories: t0=now+%.3fs, T=%.3fs", start_delay, move_time);
    }

    bool position_check(std::string joint_name,double joint_position,double minv,double maxv,SetArm::Response::SharedPtr response){
        if(joint_position < minv || joint_position > maxv){
            response->result = SetArm::Response::FAIL;
            std::ostringstream oss;
            oss << joint_name << "_angle should be between "
                << std::fixed << std::setprecision(2) << minv
                << " and " << maxv;
            response->message = oss.str();
            RCLCPP_WARN(this->get_logger(),"%s",oss.str().c_str());
            return false;
        }
        return true;
    }

    void SetArmPosition(const SetArm::Request::SharedPtr request,SetArm::Response::SharedPtr response){
        double move_time  = request->duration > 0.0 ?request->duration : get_parameter("move_time").as_double(); //获取运动历经
        double start_delay = get_parameter("start_delay").as_double();
        double joint1_angle = request->joint1_position;
        double joint2_angle = request->joint2_position;
        double joint3_angle = request->joint3_position;
        double joint4_angle = request->joint4_position;

        if(!position_check("joint1",joint1_angle,-1.58,1.58,response))return ;
        if(!position_check("joint2",joint2_angle,-1.58,1.58,response))return ;
        if(!position_check("joint3",joint3_angle,-1.58,1.58,response))return ;
        if(!position_check("joint4",joint4_angle,-1.58,1.58,response))return ;

        this->publishSynced(joint1_angle,joint2_angle,joint3_angle,joint4_angle,move_time,start_delay);

        response->result = SetArm::Response::SUCESS;
        response->message = "arm set ok";
        RCLCPP_INFO(this->get_logger(),"arm set ok");
    }
};


int main(int argc,char** argv){
  rclcpp::init(argc,argv);
  auto node = std::make_shared<ArmServer>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}